/**
 ****************************(C) COPYRIGHT 2019 DJI****************************
 * @file       INS_task.c/h
 * @brief      use bmi088 to calculate the euler angle. no use ist8310, so only
 *             enable data ready pin to save cpu time.enalbe bmi088 data ready
 *             enable spi DMA to save the time spi transmit
 *             ��Ҫ����������bmi088��������ist8310�������̬���㣬�ó�ŷ���ǣ�
 *             �ṩͨ��bmi088��data ready �ж�����ⲿ�������������ݵȴ��ӳ�
 *             ͨ��DMA��SPI�����ԼCPUʱ��.
 * @note
 * @history
 *  Version    Date            Author          Modification
 *  V1.0.0     Dec-26-2018     RM              1. done
 *  V2.0.0     Nov-11-2019     RM              1. support bmi088, but don't support mpu6500
 *
 @verbatim
 ==============================================================================

 ==============================================================================
 @endverbatim
 ****************************(C) COPYRIGHT 2019 DJI****************************
 */

#include "INS_task.h"
#include "bsp_spi.h"
#include "main.h"
#include "function.h"
#include "cmsis_os.h"
#include "gimbal.h"

#include "bmi088driver.h"
#include "ist8310driver.h"
#include "pid.h"

#include "MahonyAHRS.h"
#include "math.h"

#define IMU_temp_PWM(pwm)  imu_pwm_set(pwm)                    //pwm����

/**
 * @brief          control the temperature of bmi088
 * @param[in]      temp: the temperature of bmi088
 * @retval         none
 */
/**
 * @brief          ����bmi088���¶�
 * @param[in]      temp:bmi088���¶�
 * @retval         none
 */
static void imu_temp_control(fp32 temp);

/**
 * @brief          open the SPI DMA accord to the value of imu_update_flag
 * @param[in]      none
 * @retval         none
 */
/**
 * @brief          ����imu_update_flag��ֵ����SPI DMA
 * @param[in]      temp:bmi088���¶�
 * @retval         none
 */
static void imu_cmd_spi_dma(void);

void AHRS_init(fp32 quat[4], fp32 accel[3], fp32 mag[3]);
void AHRS_update(fp32 quat[4], fp32 time, fp32 gyro[3], fp32 accel[3],
		fp32 mag[3]);
void get_angle(fp32 quat[4], fp32 *yaw, fp32 *pitch, fp32 *roll);

extern SPI_HandleTypeDef hspi1;
extern gimbal_t gimbal;

static TaskHandle_t INS_task_local_handler;

uint8_t gyro_dma_rx_buf[SPI_DMA_GYRO_LENGHT];
uint8_t gyro_dma_tx_buf[SPI_DMA_GYRO_LENGHT] =
{ 0x82, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

uint8_t accel_dma_rx_buf[SPI_DMA_ACCEL_LENGHT];
uint8_t accel_dma_tx_buf[SPI_DMA_ACCEL_LENGHT] =
{ 0x92, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };

uint8_t accel_temp_dma_rx_buf[SPI_DMA_ACCEL_TEMP_LENGHT];
uint8_t accel_temp_dma_tx_buf[SPI_DMA_ACCEL_TEMP_LENGHT] =
{ 0xA2, 0xFF, 0xFF, 0xFF };

volatile uint8_t gyro_update_flag = 0;
volatile uint8_t accel_update_flag = 0;
volatile uint8_t accel_temp_update_flag = 0;
volatile uint8_t mag_update_flag = 0;
volatile uint8_t imu_start_dma_flag = 0;

bmi088_real_data_t bmi088_real_data;
ist8310_real_data_t ist8310_real_data;

static uint8_t first_temperate;
static const fp32 imu_temp_PID[3] =
{ TEMPERATURE_PID_KP, TEMPERATURE_PID_KI, TEMPERATURE_PID_KD };
static PID_t imu_temp_pid;

fp32 INS_quat[4] =
{ 0.0f, 0.0f, 0.0f, 0.0f };
fp32 INS_angle[3] =
{ 0.0f, 0.0f, 0.0f };      //euler angle, unit rad.ŷ���� ��λ rad
IMU_t IMU_angle;
extern TIM_HandleTypeDef htim10;
void imu_pwm_set(uint16_t pwm)
{
	__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
}
/**
 * @brief          imu task, init bmi088, ist8310, calculate the euler angle
 * @param[in]      pvParameters: NULL
 * @retval         none
 */
/**
 * @brief          imu����, ��ʼ�� bmi088, ist8310, ����ŷ����
 * @param[in]      pvParameters: NULL
 * @retval         none
 */
void INS_task(void const *pvParameters)
{
	//wait a time

	osDelay(INS_TASK_INIT_TIME);
IMU_angle.HalfRound=0;
	while (BMI088_init())
	{
		Toggle_LED_G
		;
		osDelay(100);
	}
	while (ist8310_init())
	{
		Toggle_LED_G
		;
		osDelay(100);
	}
	Toggle_LED_G
	;
	BMI088_read(bmi088_real_data.gyro, bmi088_real_data.accel,
			&bmi088_real_data.temp);

	PID_Init(&imu_temp_pid, PID_POSITION, imu_temp_PID, TEMPERATURE_PID_MAX_OUT,
			TEMPERATURE_PID_MAX_IOUT);

	AHRS_init(INS_quat, bmi088_real_data.accel, ist8310_real_data.mag);

	//get the handle of task

	INS_task_local_handler = xTaskGetHandle(pcTaskGetName(NULL));

	//set spi frequency
	hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;

	if (HAL_SPI_Init(&hspi1) != HAL_OK)
	{
		Error_Handler();
	}

	SPI1_DMA_init((uint32_t) gyro_dma_tx_buf, (uint32_t) gyro_dma_rx_buf,
			SPI_DMA_GYRO_LENGHT);

	imu_start_dma_flag = 1;

	while (1)
	{
		Toggle_LED_G
		;
		//wait spi DMA tansmit done
		//�ȴ�SPI DMA����
		while (ulTaskNotifyTake(pdTRUE, portMAX_DELAY) != pdPASS)
		{
		}

		if (gyro_update_flag & (1 << IMU_NOTIFY_SHFITS))
		{
			gyro_update_flag &= ~(1 << IMU_NOTIFY_SHFITS);
			BMI088_gyro_read_over(
					gyro_dma_rx_buf + BMI088_GYRO_RX_BUF_DATA_OFFSET,
					bmi088_real_data.gyro);
		}

		if (accel_update_flag & (1 << IMU_UPDATE_SHFITS))
		{
			accel_update_flag &= ~(1 << IMU_UPDATE_SHFITS);
			BMI088_accel_read_over(
					accel_dma_rx_buf + BMI088_ACCEL_RX_BUF_DATA_OFFSET,
					bmi088_real_data.accel, &bmi088_real_data.time);
		}

		if (accel_temp_update_flag & (1 << IMU_UPDATE_SHFITS))
		{
			accel_temp_update_flag &= ~(1 << IMU_UPDATE_SHFITS);
			BMI088_temperature_read_over(
					accel_temp_dma_rx_buf + BMI088_ACCEL_RX_BUF_DATA_OFFSET,
					&bmi088_real_data.temp);
			imu_temp_control(bmi088_real_data.temp);
		}

		AHRS_update(INS_quat, 0.001f, bmi088_real_data.gyro,
				bmi088_real_data.accel, ist8310_real_data.mag);
		get_angle(INS_quat, INS_angle + INS_YAW_ADDRESS_OFFSET,
				INS_angle + INS_PITCH_ADDRESS_OFFSET,
				INS_angle + INS_ROLL_ADDRESS_OFFSET);

		IMU_dataupdate(&IMU_angle,INS_angle);

	}
}

void AHRS_init(fp32 quat[4], fp32 accel[3], fp32 mag[3])
{
	quat[0] = 1.0f;
	quat[1] = 0.0f;
	quat[2] = 0.0f;
	quat[3] = 0.0f;

}

void AHRS_update(fp32 quat[4], fp32 time, fp32 gyro[3], fp32 accel[3],
		fp32 mag[3])
{
	// MahonyAHRSupdate(quat, gyro[0], gyro[1], gyro[2], accel[0], accel[1], accel[2], mag[0], mag[1], mag[2]);
	MahonyAHRSupdate(quat, gyro[0], gyro[1], gyro[2], accel[0], accel[1],
			accel[2], 0, 0, 0);
}
void get_angle(fp32 q[4], fp32 *yaw, fp32 *pitch, fp32 *roll)
{
	*yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]),
			2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f);
	*pitch = asinf(-2.0f * (q[1] * q[3] - q[0] * q[2]));
	*roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]),
			2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f);
}

/**
 * @brief          control the temperature of bmi088
 * @param[in]      temp: the temperature of bmi088
 * @retval         none
 */
/**
 * @brief          ����bmi088���¶�
 * @param[in]      temp:bmi088���¶�
 * @retval         none
 */
static void imu_temp_control(fp32 temp)
{
	uint16_t tempPWM;
	static uint8_t temp_constant_time = 0;
	if (first_temperate)
	{
		PID_Calc(&imu_temp_pid, temp, 45.0f);
		if (imu_temp_pid.out < 0.0f)
		{
			imu_temp_pid.out = 0.0f;
		}
		tempPWM = (uint16_t) imu_temp_pid.out;
		IMU_temp_PWM(tempPWM);
	}
	else
	{
		//��û�дﵽ���õ��¶ȣ�һֱ����ʼ���
		//in beginning, max power
		if (temp > 45.0f)
		{
			temp_constant_time++;
			if (temp_constant_time > 200)
			{
				//�ﵽ�����¶ȣ�������������Ϊһ������ʣ���������
				//
				first_temperate = 1;
				imu_temp_pid.Iout = MPU6500_TEMP_PWM_MAX / 2.0f;
			}
		}

		IMU_temp_PWM(MPU6500_TEMP_PWM_MAX - 1);
	}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if (GPIO_Pin == INT1_ACCEL_Pin)
	{
		accel_update_flag |= 1 << IMU_DR_SHFITS;
		accel_temp_update_flag |= 1 << IMU_DR_SHFITS;
		if (imu_start_dma_flag)
		{
			imu_cmd_spi_dma();
		}
	}
	else if (GPIO_Pin == INT1_GYRO_Pin)
	{
		gyro_update_flag |= 1 << IMU_DR_SHFITS;
		if (imu_start_dma_flag)
		{
			imu_cmd_spi_dma();
		}
	}
//    else if(GPIO_Pin == DRDY_IST8310_Pin)
//    {
//        mag_update_flag |= 1 << IMU_DR_SHFITS;
//
//        if(mag_update_flag &= 1 << IMU_DR_SHFITS)
//        {
//            mag_update_flag &= ~(1<< IMU_DR_SHFITS);
//            mag_update_flag |= (1 << IMU_SPI_SHFITS);
//
//            ist8310_read_mag(ist8310_real_data.mag);
//        }
//    }
	else if (GPIO_Pin == GPIO_PIN_0)
	{
		//wake up the task
		//��������
		if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
		{
			static BaseType_t xHigherPriorityTaskWoken;
			vTaskNotifyGiveFromISR(INS_task_local_handler,
					&xHigherPriorityTaskWoken);
			portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
		}
	}

}

/**
 * @brief          open the SPI DMA accord to the value of imu_update_flag
 * @param[in]      none
 * @retval         none
 */
/**
 * @brief          ����imu_update_flag��ֵ����SPI DMA
 * @param[in]      temp:bmi088���¶�
 * @retval         none
 */
static void imu_cmd_spi_dma(void)
{
	UBaseType_t uxSavedInterruptStatus;
	uxSavedInterruptStatus = taskENTER_CRITICAL_FROM_ISR();

	//���������ǵ�DMA����
	if ((gyro_update_flag & (1 << IMU_DR_SHFITS))
			&& !(hspi1.hdmatx->Instance->CR & DMA_SxCR_EN)
			&& !(hspi1.hdmarx->Instance->CR & DMA_SxCR_EN)
			&& !(accel_update_flag & (1 << IMU_SPI_SHFITS))
			&& !(accel_temp_update_flag & (1 << IMU_SPI_SHFITS)))
	{
		gyro_update_flag &= ~(1 << IMU_DR_SHFITS);
		gyro_update_flag |= (1 << IMU_SPI_SHFITS);

		HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
		SPI1_DMA_enable((uint32_t) gyro_dma_tx_buf, (uint32_t) gyro_dma_rx_buf,
				SPI_DMA_GYRO_LENGHT);
		taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus);
		return;
	}
	//�������ٶȼƵ�DMA����
	if ((accel_update_flag & (1 << IMU_DR_SHFITS))
			&& !(hspi1.hdmatx->Instance->CR & DMA_SxCR_EN)
			&& !(hspi1.hdmarx->Instance->CR & DMA_SxCR_EN)
			&& !(gyro_update_flag & (1 << IMU_SPI_SHFITS))
			&& !(accel_temp_update_flag & (1 << IMU_SPI_SHFITS)))
	{
		accel_update_flag &= ~(1 << IMU_DR_SHFITS);
		accel_update_flag |= (1 << IMU_SPI_SHFITS);

		HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
		SPI1_DMA_enable((uint32_t) accel_dma_tx_buf,
				(uint32_t) accel_dma_rx_buf, SPI_DMA_ACCEL_LENGHT);
		taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus);
		return;
	}

	if ((accel_temp_update_flag & (1 << IMU_DR_SHFITS))
			&& !(hspi1.hdmatx->Instance->CR & DMA_SxCR_EN)
			&& !(hspi1.hdmarx->Instance->CR & DMA_SxCR_EN)
			&& !(gyro_update_flag & (1 << IMU_SPI_SHFITS))
			&& !(accel_update_flag & (1 << IMU_SPI_SHFITS)))
	{
		accel_temp_update_flag &= ~(1 << IMU_DR_SHFITS);
		accel_temp_update_flag |= (1 << IMU_SPI_SHFITS);

		HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
		SPI1_DMA_enable((uint32_t) accel_temp_dma_tx_buf,
				(uint32_t) accel_temp_dma_rx_buf, SPI_DMA_ACCEL_TEMP_LENGHT);
		taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus);
		return;
	}
	taskEXIT_CRITICAL_FROM_ISR(uxSavedInterruptStatus);
}

void DMA2_Stream2_IRQHandler(void)
{

	if (__HAL_DMA_GET_FLAG(hspi1.hdmarx,
			__HAL_DMA_GET_TC_FLAG_INDEX(hspi1.hdmarx))
			!= RESET)
	{
		__HAL_DMA_CLEAR_FLAG(hspi1.hdmarx,
				__HAL_DMA_GET_TC_FLAG_INDEX(hspi1.hdmarx));

		//gyro read over
		//�����Ƕ�ȡ���
		if (gyro_update_flag & (1 << IMU_SPI_SHFITS))
		{
			gyro_update_flag &= ~(1 << IMU_SPI_SHFITS);
			gyro_update_flag |= (1 << IMU_UPDATE_SHFITS);

			HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);

		}

		//accel read over
		//���ٶȼƶ�ȡ���
		if (accel_update_flag & (1 << IMU_SPI_SHFITS))
		{
			accel_update_flag &= ~(1 << IMU_SPI_SHFITS);
			accel_update_flag |= (1 << IMU_UPDATE_SHFITS);

			HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
		}
		//temperature read over
		//�¶ȶ�ȡ���
		if (accel_temp_update_flag & (1 << IMU_SPI_SHFITS))
		{
			accel_temp_update_flag &= ~(1 << IMU_SPI_SHFITS);
			accel_temp_update_flag |= (1 << IMU_UPDATE_SHFITS);

			HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
		}

		imu_cmd_spi_dma();

		if (gyro_update_flag & (1 << IMU_UPDATE_SHFITS))
		{
			gyro_update_flag &= ~(1 << IMU_UPDATE_SHFITS);
			gyro_update_flag |= (1 << IMU_NOTIFY_SHFITS);
			__HAL_GPIO_EXTI_GENERATE_SWIT(GPIO_PIN_0);
		}
	}
}

void IMU_dataupdate(IMU_t* IMU, float INS[3])
{
	IMU->yaw = INS[0] * rad2angle;
	IMU->pitch = INS[1] * rad2angle;
	IMU->roll = INS[2] * rad2angle;
	if ((IMU->yaw - IMU->last_yaw) > 50.0f)
	{
		IMU->HalfRound--;
	}
	if ((IMU->yaw - IMU->last_yaw) < -50.0f)
	{
		IMU->HalfRound++;
	}
	IMU->last_yaw = IMU->yaw;
	IMU->angle = IMU->HalfRound * 360.0f + IMU->yaw;
}
